/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */

package edu.wpi.first.wpilibj.templates;

import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.SmartDashboard;

/**
 *
 * @author Administrator
 */
public class LineFollower {
    private DigitalInput m_lineSensor[] = new DigitalInput[3];
    private static final double kSpeed = .5;
    private double m_leftMotorSpeed = kSpeed;
    private double m_rightMotorSpeed = kSpeed;
    private int m_bSensors = 0;
    private static final double kAdjustSoft = 0.003;
    private static final double kAdjustHard = 0.006;

    public LineFollower(int slot, int lineIdx1, int lineIdx2, int lineIdx3) {
        m_lineSensor[0] = new DigitalInput(slot, lineIdx1);
        m_lineSensor[1] = new DigitalInput(slot, lineIdx2);
        m_lineSensor[2] = new DigitalInput(slot, lineIdx3);
    }

    private int getInput() {
        int base10 = 0;
        for (int i = 0; i < 3; i++) {
            if (!m_lineSensor[i].get()) {
                base10 += 1 << i;
            }
        }
        return base10;
    }

    private void Tadjust(boolean goLeft) {
        m_leftMotorSpeed = 0;
        m_rightMotorSpeed = 0;
        if (!goLeft) {
            m_leftMotorSpeed = kSpeed;
        }
        else {
            m_rightMotorSpeed = kSpeed;
        }
    }

    public boolean Follow(RobotDrive driveTrain, boolean goLeft) {
        if (m_bSensors != getInput())
            DriverStationPrinter.getInstance().println(String.valueOf(!m_lineSensor[0].get()) + String.valueOf(!m_lineSensor[1].get()) + String.valueOf(!m_lineSensor[2].get()));
        m_bSensors = getInput();
        switch (getInput()) {
            case 0:     // 000 - No line, at T.
                Tadjust(goLeft);
                break;
            case 1:     // 001 - Very far left
                m_leftMotorSpeed += kAdjustHard;
                m_rightMotorSpeed -= kAdjustHard;
                break;
            case 2:     // 010 - Middle
                m_leftMotorSpeed = kSpeed;
                m_rightMotorSpeed = kSpeed;
                break;
            case 3:     // 011 - Left
                m_leftMotorSpeed += kAdjustSoft;
                m_rightMotorSpeed -= kAdjustSoft;
                break;
            case 4:     // 100 - Very far right
                m_leftMotorSpeed -= kAdjustHard;
                m_rightMotorSpeed += kAdjustHard;
                break;
            case 5:     // 101 - At T
                Tadjust(goLeft);
                break;
            case 6:     // 110 - Right
                m_leftMotorSpeed -= kAdjustSoft;
                m_rightMotorSpeed += kAdjustSoft;
                break;
            case 7:     // 111 - At T
                Tadjust(goLeft);
                break;
        }
        driveTrain.setLeftRightMotorOutputs(-m_leftMotorSpeed, -m_rightMotorSpeed);
        return false;
    }
    public void updateSmartDashboard()
    {
        SmartDashboard.log(m_lineSensor[0].get(), "Right Line");
        SmartDashboard.log(m_lineSensor[1].get(), "Middle Line");
        SmartDashboard.log(m_lineSensor[2].get(), "Left Line");
    }
}
